home *** CD-ROM | disk | FTP | other *** search
/ Aminet 7 / Aminet 7 - August 1995.iso / Aminet / misc / sci / RARS_Amiga_3.lha / RARS / wappucar.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-27  |  20.5 KB  |  693 lines
  1. // This is robot driver 'WappuCar' by Jussi Pajala, May, 1995
  2.  
  3. // This is slightly modified version of Wappucar which won the second
  4. // place in april 30 races. I have already done some translation to
  5. // english ( only most important variable and function names).
  6. // There is also some english comments.
  7.  
  8. #include <string.h>
  9. #include <stdlib.h>
  10. #include <math.h>
  11. //#include <graphics.h>
  12. #include "car.h"
  13. #include "track.h"
  14.  
  15. #define false 0
  16. #define true 1
  17. #define pii 3.14159265359
  18.  
  19. // curve types
  20. #define straight 0    // straight segment
  21. #define norm_curv 1 // normal curve, straight before and after it
  22. #define first_curv 2 // straight before and curve to same direction after it
  23. #define last_curv 3 // last curve of this direction, straight after it
  24. #define mid_curv 4 // in the middle of the curves of same direction
  25. #define norm_shic 5 // shicane; curve to the other direction before and
  26.             // straight or shicane after it
  27. #define last_curv_shic 6 // some curves of the other direction before and
  28.              // straight or shicane after it
  29. #define shic_first_curv 7 // a curve of the other before and some
  30.               // curves of same direction after it
  31. #define last_curv_shic_first_curv 8 // some curves of the other direction
  32.                     // before and some of the other direction
  33.                     // after
  34.  
  35. extern const double CARWID;
  36.  
  37. extern int NSEG;              // number of segments
  38. extern segment *trackout;     // track defining arrays - see TRACK.CPP
  39. extern segment *trackin;
  40. extern double width;
  41. extern char trackfile[];
  42. extern int no_display;
  43.  
  44. const char name[] = "WappuCar";
  45.  
  46. // parameters for each track
  47. const char tracks[8][16]={ "zandvort.trk","oval2.trk","stef2.trk","speed2.trk",
  48.                "v03.trk","anew.trk","fourmile.trk","4mile"};
  49. const int known_tracks=8;
  50. const double consts[8][10]={{31,25,5.5 , .15,.25,.15,.7,.2, 50, 5},// zandvort
  51.                 {31,25,5.9 , .15,.25,.15,.8,.2, 35,.5},// oval2
  52.                 {34,30,5.65, .15,.25,.15, 0,.1, 55, 5},// stef2
  53.                 {33,25,5.6 , .15,.25,.15,.8,.2, 35, 1},// speed2
  54.                 {31,25,5.65, .15,.25,.15, 1,.2, 20, 0},// v03
  55.                 {30,30,5.62, .15,.34,.23,.8,.15,27,12},// anew
  56.                 {31,20,5.75, .15,.25,.15,.7,.2 ,65, 0},// fourmile
  57.                 {31,20,5.75, .15,.25,.15,.7,.2 ,65, 0}};// 4mile
  58.  
  59. // parameters :
  60. static double a=31;      // acceleration on straight
  61. static double kurv_a=25; // acceleration on curve
  62. static double v_kerr=5.6;// speed rate
  63. static double accslad=.15,decslad=.25;// max acceleration and braking slide percent
  64. static double curvslad=.15; // max slide percent on curve
  65. static double ajolinjakerr=.7;// braking rate when not on driving line
  66. static double maxkulma=.2;    // max angle to drive to the line
  67. static double vara=35;        // space to outer curve
  68. static double sisavara=7;    // space to inner curve
  69.  
  70. // v_kerr, vara and sisavara are the most important parameters
  71.  
  72. static double vc=1000,alpha=0;
  73. static int init_flag=true;
  74.  
  75. con_vec result;
  76. double r1,r2;              // curves inner and outer radian ( with space )
  77. double nex_r1,nex_r2;      // and the same for next curve
  78. double r3,nex_r3;          // driving lines radian
  79. double nex_yli;            // how far the driving line is from end of straight
  80. double v0,x;               // speed on next curve, x=distance to curve
  81. static int ulkona=false;   // if out of road during last round
  82. static int tracknum=0;     // current tracks number
  83. static int n;              // current segments number
  84. static int ohitus=0;       // <>0 when passing if <0 passing from right side
  85. static int firstround=true;// true when called first time
  86.  
  87. static char trackname[16];
  88.  
  89. struct ratataul {
  90.   int tyyppi;    //curve type
  91.   double ajor;   //driving lines rad
  92.   double r;      // curves rad
  93.   double pituus; //length
  94. };
  95. ratataul *curves;
  96.  
  97.  
  98. /*void piirramit(double v){
  99. #define xx 250
  100. #define yy 50
  101. #define mit_r 40
  102. static int mx=xx,my=yy;
  103.   setcolor(2);
  104.   line(xx,yy,mx,my);
  105.   my=yy+mit_r*cos(v*.021);
  106.   mx=xx-mit_r*sin(v*.021);
  107.   setcolor(0);
  108.   line(xx,yy,mx,my);
  109. } */
  110.  
  111. double wc_driving_lines_rad(int tyyppi,double r,double omega){
  112. double r1,r2=0;
  113.  
  114.   // outer and inner curve with spaces
  115.   if (r<0){ r1=r-sisavara; r2=r-width+vara;}
  116.   else { r1=r+sisavara; r2=r+width-vara; }
  117.  
  118.   // driving lines radian
  119.   switch(tyyppi){
  120.  
  121.   case norm_curv: return (r2-r1*cos(omega/2))/(1-cos(omega/2));// kaava 1;
  122.   case norm_shic: return (r2-r1*cos(omega/2))/(1-cos(omega/2));
  123.  
  124.   case first_curv:                return (r2-r1*cos(omega))/(1-cos(omega));
  125.   case shic_first_curv:           return (r2-r1*cos(omega))/(1-cos(omega));
  126.   case last_curv_shic_first_curv: return (r2-r1*cos(omega))/(1-cos(omega));
  127.  
  128.   case last_curv:      return (r2-r1*cos(omega))/(1-cos(omega));
  129.   case last_curv_shic: return (r2-r1*cos(omega))/(1-cos(omega));
  130.  
  131.   case mid_curv:return r1;
  132.   default:return 0;
  133.   }
  134. }
  135.  
  136. void wc_track_init(void){
  137. int i=0;
  138.   firstround=false;n=0;
  139.   strcpy(trackname,trackfile);       // vain 1. kierroksella
  140.  
  141.   for(tracknum=0; tracknum<known_tracks; tracknum++)
  142.     if (strcmpi(tracks[tracknum],trackname)==0) break;
  143.   if (tracknum<known_tracks){
  144.     a=consts[tracknum][0];
  145.     kurv_a=consts[tracknum][1];
  146.     v_kerr=consts[tracknum][2];
  147.     accslad=consts[tracknum][3];
  148.     decslad=consts[tracknum][4];
  149.     curvslad=consts[tracknum][5];
  150.     ajolinjakerr=consts[tracknum][6];
  151.     maxkulma=consts[tracknum][7];
  152.     vara=consts[tracknum][8];
  153.     sisavara=consts[tracknum][9];
  154.   }
  155.  
  156.  
  157.   curves=new ratataul[NSEG+3];
  158.  
  159.   for (i=0;i<NSEG;i++){  // count inner radian of the curve:
  160.     if (trackin[i].radius==0&&trackout[i].radius==0) curves[i].r=0;
  161.     else if (trackin[i].radius<0) curves[i].r=trackout[i].radius;
  162.     else curves[i].r=trackin[i].radius;
  163.     curves[i].pituus=trackin[i].length;
  164.   }
  165.   curves[NSEG]=curves[0]; // copy first radian to last
  166.  
  167.   // identify curve types:
  168.   for(i=0;i<NSEG;i++){
  169.     if (curves[i].r==0) curves[i].tyyppi=straight;
  170.  
  171.     else if (curves[i].r*curves[i-1].r>0) // if curve before is to same direction
  172.       if (curves[i].r*curves[i+1].r>0) curves[i].tyyppi = mid_curv;
  173.       else if (curves[i].r*curves[i+1].r<0)
  174.     curves[i].tyyppi = last_curv_shic;
  175.       else curves[i].tyyppi = last_curv;
  176.  
  177.     else if (curves[i].r*curves[i-1].r==0)  // if segment before is straight
  178.       if (curves[i].r*curves[i+1].r>0) curves[i].tyyppi = first_curv;//next is to same direction
  179.       else if (curves[i].r*curves[i+1].r<0) //if next is to the other direction
  180.     curves[i].tyyppi = norm_shic;
  181.       else curves[i].tyyppi = norm_curv; //if next is straight too
  182.  
  183.     else  // if curve before is to the other direction
  184.       if (curves[i].r*curves[i+1].r>0) //if next is to same dir
  185.     if (curves[i+1].r*curves[i+2].r>0) // and next one too
  186.       if (curves[i-1].tyyppi==last_curv_shic)
  187.          //if curve before was the last of the same direction
  188.         curves[i].tyyppi=last_curv_shic_first_curv;
  189.       else curves[i].tyyppi=shic_first_curv;//if curve before is norm_curv or shicane
  190.     else curves[i].tyyppi = shic_first_curv;
  191.       else if (curves[i].r*curves[i+1].r<0)
  192.     curves[i].tyyppi = norm_shic; //curve before and after to the other direction
  193.       else curves[i].tyyppi = norm_shic; //curve before to other direction,
  194.                      // straight after
  195.   }
  196.  
  197.   // driving lines radian
  198.   for(i=0;i<NSEG;i++){
  199.     switch(curves[i].tyyppi){
  200.       case straight: curves[i].ajor=0; break;
  201.       default: curves[i].ajor
  202.            = wc_driving_lines_rad(curves[i].tyyppi,curves[i].r,trackin[i].length);
  203.     }
  204.   }
  205.   curves[NSEG]=curves[0];   // copy track from the beginning to the end
  206.   curves[NSEG+1]=curves[1];
  207.   curves[NSEG+2]=curves[2];
  208.  
  209.  
  210. }
  211.  
  212.  
  213. double wc_abs(double x){
  214.   if (x<0) return -x; else return x;
  215. }
  216.  
  217. double wc_sqr(double x){
  218.   return x*x;
  219. }
  220.  
  221. double wc_minimum(double a,double b){
  222.   if (a<b) return a;
  223.   else return b;
  224. }
  225.  
  226.  
  227. double wc_laske_yli(int tyyppi,double r1,double r2,double r3,double omega){
  228. // this routine calculates how far on the straight the driving line begins
  229.   double d;
  230.   switch(tyyppi){
  231.     case norm_curv:
  232.     case norm_shic:
  233.       d=sqrt(wc_sqr(r2)+wc_sqr(r3-r1)-2*r2*(r3-r1)*cos(pii-omega/2));// kaava 2
  234.       break;
  235.     case first_curv:
  236.     case last_curv:
  237.     case last_curv_shic:
  238.     case shic_first_curv:
  239.     case last_curv_shic_first_curv:
  240.       d=sqrt(r2*r2+(r3-r1)*(r3-r1)-2*r2*(r3-r1)*cos(3.142-omega));// kaava 2
  241.       break;
  242.     default:d=r3;
  243.   }
  244.   return sqrt(d*d-r3*r3);  // kaava 3
  245. }
  246.  
  247.  
  248. void wc_laskeperusjutut(situation s){
  249.  
  250.   r1=r2=nex_r1=nex_r2=0;  // nollataan s„teet
  251.  
  252.   // lasketaan kurvin sis„- ja ulkos„de
  253.   if (s.cur_rad<0){
  254.       r2=s.cur_rad-width+vara;
  255.       r1=s.cur_rad-sisavara;
  256.   }
  257.   else if (s.cur_rad>0){
  258.     r2=s.cur_rad+width-vara;
  259.     r1=s.cur_rad+sisavara;
  260.   }
  261.   // samoin sis„- ja ulkos„de seuraavalle kurville
  262.   if (s.nex_rad<0){
  263.       nex_r2=s.nex_rad-width+vara;
  264.       nex_r1=s.nex_rad-sisavara;
  265.   }
  266.   else if (s.nex_rad>0){
  267.     nex_r2=s.nex_rad+width-vara;
  268.     nex_r1=s.nex_rad+sisavara;
  269.   }
  270. }
  271.  
  272.  
  273. void wc_calc_driving_line(situation s){  // lasketaan ajolinjan s„de ja ylitys, jos mahd.
  274.   if (s.cur_rad==0) r3=0;
  275.   else r3=curves[n].ajor;
  276.  
  277.   if (s.nex_rad==0){ nex_r3=0;nex_yli=0;}
  278.   else {
  279.     nex_r3=curves[n+1].ajor; // ajor = driving lines radian
  280.     nex_yli=wc_laske_yli(curves[n+1].tyyppi,nex_r1,nex_r2,nex_r3,s.nex_len);
  281.         // see wc_laske_yli function
  282.   }
  283. }
  284.  
  285.  
  286. void wc_speed(situation s){
  287.  
  288.   vc=1000;
  289.   // nopeus suoralla, ja mutkaan tullessa
  290.   if (r3==0){
  291.     if (nex_r3!=0){
  292.        x=s.to_end-nex_yli;
  293.        if (x<0) x=0;
  294.        v0=v_kerr*sqrt(wc_abs(nex_r3));
  295.        wc_minimum(vc=sqrt(v0*v0+2*a*x),vc);
  296.  
  297.        if (wc_abs(curves[n+2].ajor)<wc_abs(curves[n+1].ajor)&&curves[n+2].r!=0){
  298.      x=s.to_end+wc_abs(s.nex_rad*s.nex_len); // et„isyys mutkan alkuun
  299.          v0=v_kerr*sqrt(wc_abs(curves[n+2].ajor));
  300.          vc=wc_minimum(sqrt(v0*v0+2*kurv_a*x),vc);
  301.        }
  302.        // tai sit„ seuraava
  303.        if (wc_abs(curves[n+3].ajor)<wc_abs(curves[n+1].ajor)&&curves[n+3].r!=0){
  304.      x=s.to_end+wc_abs(s.nex_rad*s.nex_len); // et„isyys seur. mutkan
  305.                       // takaisen palikan alkuun
  306.      if (s.after_rad!=0) x+=wc_abs(s.after_rad)*curves[n+2].pituus;
  307.      else x+=curves[n+2].pituus;
  308.  
  309.          v0=v_kerr*sqrt(wc_abs(curves[n+3].ajor));
  310.          vc=wc_minimum(sqrt(v0*v0+2*kurv_a*x),vc);
  311.        }
  312.     }
  313.   }
  314.   else { // nopeus mutkassa, tiukempaan mutkaan tullessa
  315.     if ((wc_abs(nex_r3)<wc_abs(r3)&&wc_abs(nex_r3)!=0)){ //jos seuraava kurvi tiukempi
  316.       x=s.to_end*wc_abs(s.cur_rad)-nex_yli;  // vois ehk„ v„h„n tarkentaa, et„lopp ehk„ v„h„n
  317.               // turhan pitk„ ( s.cur_rad*s.to_end )
  318.       if (x<0) x=0;
  319.       v0=v_kerr*sqrt(wc_abs(nex_r3));
  320.       vc=sqrt(v0*v0+2*kurv_a*x);
  321.     }
  322.     else if(wc_abs(curves[n+2].ajor)<wc_abs(r3)&&curves[n+2].r!=0){//nopeus mutkassa,
  323.                       // kun seuraavan seuraava kurva tiukempi
  324.       x=s.to_end*wc_abs(s.cur_rad);
  325.  
  326.       if (s.nex_rad!=0)  x+=wc_abs(s.nex_rad)*s.nex_len;
  327.       else x+=s.nex_len;
  328.  
  329.       v0=v_kerr*sqrt(wc_abs(s.after_rad)+width);
  330.       vc=sqrt(wc_sqr(v0)+2*kurv_a*x);
  331.     }
  332.     vc=wc_minimum( vc,v_kerr*sqrt(wc_abs(r3)) );  // nopeus mutkassa
  333.   }
  334.  
  335. }
  336. double wc_anna_fii(int tyyppi,double pit,double loppuun){
  337. double fii=0;
  338.   // fii on "autosta kurvin keskipisteeseen ja kurvin keskipisteest„
  339.   // ajos„teen keskipisteeseen" v„linen kulma
  340.  
  341.   switch(tyyppi){
  342.     case first_curv                : fii = loppuun;break;
  343.     case shic_first_curv           : fii = loppuun;break;
  344.     case last_curv_shic_first_curv : fii = loppuun;break;
  345.  
  346.     case last_curv      : fii = pit-loppuun;break;
  347.     case last_curv_shic : fii = pit-loppuun;break;
  348.  
  349.  
  350.     case norm_curv : fii = 0.5*pit-loppuun;break;
  351.     case norm_shic : fii = 0.5*pit-loppuun;break;
  352.     case mid_curv : fii = 0.5*pit-loppuun;break;
  353.  
  354.     default        : fii = 0;
  355.   }
  356.   if (fii>0) fii=pii-fii;
  357.   else fii=pii+fii;
  358.   return fii;
  359. }
  360.  
  361. double wc_anna_theta(situation s){
  362.   // theta on kurvin suunnan ja ajoympyr„n tangentin v„linen kulma
  363.  
  364.   double A,B,C,D,E;
  365.   double alfa,beta,epsilon,zeta,fii,theta;
  366.   double v0,x,y; // v0 ja x nopeuden laskemisessa, x ja y peilattaessa
  367.          // shikaania suoralle
  368.  
  369.   if (s.cur_rad!=0){
  370.   //Jos ollaan kurvissa suoritetaan t„m„ :
  371.     A=r3-r1;if (A<0) A=-A;
  372.  
  373.     if (s.cur_rad>0)B=s.cur_rad+s.to_lft; // B = et. kurvin keskustaan
  374.     else B=-s.cur_rad+s.to_rgt;
  375.  
  376.     fii=wc_anna_fii(curves[n].tyyppi,s.cur_len,s.to_end);
  377.  
  378.     C=sqrt(wc_sqr(A)+wc_sqr(B)-2*A*B*cos(fii));
  379.  
  380.     // Jos ollaan ajolinjan sis„puolella palautetaan nolla
  381.     if (C<r3 || ( r3<0&&C<-r3)) epsilon=0;
  382.  
  383.     // Jos ollaan ajolinjan ulkopuolella jatketaan laskemista
  384.     else {
  385.       if (curves[n].tyyppi!=mid_curv){
  386.  
  387.       //jos tyyppi = 'mid_curv'  ei tarvita n„it„:
  388.     zeta=acos( (wc_sqr(B)-wc_sqr(C)-wc_sqr(A))/(-2*A*C) ); // zeta = kulma(A,C)
  389.  
  390.     alfa=r3/C;if (alfa<0) alfa=-alfa;
  391.     alfa=acos(alfa);if (alfa<0) alfa=-alfa; // alfa = kulma(C,r3)
  392.  
  393.     if (s.to_end>.5*s.cur_len&&
  394.         (s.cur_len-s.to_end)+zeta<.5*s.cur_len)
  395.       beta=zeta-alfa;
  396.     else if (s.to_end>.5*s.cur_len) beta=alfa-zeta;
  397.     else beta=alfa+zeta;// alfa+zeta
  398.  
  399.     if (r3>0) E=sqrt(wc_sqr(r3)+wc_sqr(A)-2*A*r3*cos(beta));
  400.     else E=sqrt(wc_sqr(r3)+wc_sqr(A)-2*A*(-r3)*cos(beta));
  401.       } else  E=r3;
  402.  
  403.       D=sqrt(wc_sqr(C)-wc_sqr(r3));
  404.       epsilon=acos( (wc_sqr(E)-wc_sqr(D)-wc_sqr(B))/(-2*D*B) );
  405.  
  406.       epsilon=.5*pii-epsilon; // oikeastaan t„m„ on jo theta
  407.     }
  408.     // Jos shikaanissa, lasketaan tangentti seuraavalle segmentille
  409.     if ((s.to_end<.5*pii)&&(curves[n].tyyppi==norm_shic||
  410. //        curves[n].tyyppi==shic_first_curv||
  411.     curves[n].tyyppi==last_curv_shic//||
  412. //        curves[n].tyyppi==last_curv_shic_first_curv)
  413.       )){
  414.  
  415.       if (s.cur_rad<0) D=-s.cur_rad+s.to_rgt;
  416.       else D=s.cur_rad+s.to_lft;
  417.  
  418.       y=wc_abs(r1)-D*cos(s.to_end);
  419.       x=D*sin(s.to_end);
  420.       if (x<nex_yli){ // jos ajolinja mahdollista laskea
  421.  
  422.     C=nex_yli-x;
  423.     alfa=atan(C/wc_abs(nex_r3));
  424.  
  425.     if (nex_r3>0){
  426.       B=nex_r3-sqrt(wc_sqr(nex_r3)-wc_sqr(C));
  427.       A=nex_r3+(B+y)*cos(alfa);
  428.     }
  429.     else {
  430.       B=-nex_r3-sqrt(wc_sqr(nex_r3)-wc_sqr(C));
  431.       A=nex_r3-(B+y)*cos(alfa);
  432.     }
  433.  
  434.     // jos ollaan ajolinjan sis„puolella
  435.     if (wc_abs(A)<wc_abs(nex_r3)) {
  436.        // Jos ollaan ajolinjan sis„puolella, v„hennet„„n nopeutta
  437.       if (ajolinjakerr!=0){ // jos nopeudenv„hennyst„ ei ole estetty
  438.         x=(s.to_end-nex_yli); // et„isyys "ajolinjan" alkuun
  439.         if (x<0) x=0;
  440.         v0=ajolinjakerr*wc_sqr(wc_sqr(A/nex_r3))*v_kerr*sqrt(wc_abs(nex_r3));
  441.         vc=sqrt(v0*v0+2*a*x);
  442.       }
  443.       return epsilon ;
  444.     }
  445.     beta=asin(nex_r3/A); if (beta<0) beta=-beta;
  446.     theta =  s.to_end - (.5*pii+alfa-beta);
  447.  
  448.     if (s.cur_rad>0)B=s.cur_rad+s.to_lft; // B = et. kurvin keskustaan
  449.     else B=-s.cur_rad+s.to_rgt;
  450.  
  451.     if (wc_abs(r1)<B) alfa=pii/2- asin(wc_abs(r1)/B);
  452.     else alfa=0;// tien keskiviivan ja sis„kurvin tangentin v„linen kulma
  453.  
  454.  
  455.     if ( ( wc_abs(theta)<wc_abs(alfa) || alfa*theta<0 )
  456. /*             && ( (r3<0&&theta<epsilon) || (r3>=0&&theta>epsilon) )*/ )
  457.       return theta;
  458. //        else if (r3>=0&&theta>epsilon) return theta;
  459.       }
  460.     }
  461.  
  462.     return epsilon ;
  463.   }
  464.  
  465.   // Jos ollaan suoralla suoritetaan seuraava:
  466.   else
  467.   {
  468.     C=nex_yli-s.to_end;
  469.     alfa=atan(C/nex_r3);if (alfa<0) alfa=-alfa;
  470.  
  471.     if (nex_r3>0){
  472.       B=nex_r3-sqrt(wc_sqr(nex_r3)-wc_sqr(C));
  473.       A=nex_r3+(B-s.to_rgt+vara)*cos(alfa);
  474.     }
  475.     else {
  476.       B=-nex_r3-sqrt(wc_sqr(nex_r3)-wc_sqr(C));
  477.       A=nex_r3-(B-s.to_lft+vara)*cos(alfa);
  478.     }
  479.  
  480.  
  481.     if ( (A<nex_r3&&nex_r3>0)||(-A<-nex_r3&&nex_r3<0)){
  482.       // Jos ollaan ajolinjan sis„puolella, v„hennet„„n nopeutta
  483.      if (ajolinjakerr!=0){ // jos nopeudenv„hennyst„ ei ole estetty
  484.        x=(s.to_end-nex_yli); // et„isyys "ajolinjan" alkuun
  485.        if (x<0) x=0;
  486.        v0=ajolinjakerr*/*wc_sqr*/(wc_sqr(A/nex_r3))*v_kerr*sqrt(wc_abs(nex_r3));
  487.        vc=sqrt(v0*v0+2*a*x);
  488.      }
  489.  
  490.      return 0; // ja ajetaan suoraan
  491.     }
  492.     else {
  493.       beta=asin(nex_r3/A); if (beta<0&&nex_r3>0) beta=-beta;
  494.       if (nex_r3>0) return .5*pii+alfa-beta;
  495.       else if (nex_r3<0) return -(.5*pii+alfa-beta);
  496.     }
  497.   }
  498.   return 999;
  499. }
  500.  
  501.  
  502. void wc_alpha(situation s){  // K„„nn”ksenlaskufunktio
  503. double suuntero,v_kulma,tan_kulma;
  504. double et=0,lev;
  505.  
  506.   if (s.cur_rad!=0){  // Lasketaan keulan suunta mutkassa
  507.     v_kulma=asin( s.vn/s.v );
  508.     tan_kulma=wc_anna_theta(s);
  509.     if (r3>0) suuntero=-v_kulma+/*kaantokerr**/tan_kulma;
  510.     else if (r3<0) suuntero=-v_kulma-/*kaantokerr**/tan_kulma;
  511.   }
  512.   if (s.cur_rad==0){  // Ja keulan suunta suoralla
  513.     if (s.to_end!=0&&s.to_end>nex_yli){ // kun kaukana kurvista...
  514.  
  515.       et=s.to_end-nex_yli;
  516.       if (s.nex_rad<0) lev=wc_abs(-s.to_lft+vara);
  517.       else lev=wc_abs(s.to_rgt-vara);
  518.       if ( wc_abs(lev/et)>maxkulma && maxkulma!=0)
  519.  
  520.       et=s.to_end;
  521.  
  522.       if (s.nex_rad>0)
  523.     suuntero=-atan((s.to_rgt-vara)/et)-asin(s.vn/s.v);
  524.       else if (s.nex_rad<0)
  525.     suuntero=-atan((-s.to_lft+vara)/et)-asin(s.vn/s.v);
  526.       else suuntero=-asin(s.vn/s.v);
  527.  
  528.       if (suuntero<-maxkulma&&s.nex_rad>0) suuntero=-maxkulma;
  529.       else if (suuntero>maxkulma&&s.nex_rad<0) suuntero=maxkulma;
  530.     }
  531.     else if (s.to_end<nex_yli){ // ... ja l„hell„ kurvia
  532.       v_kulma=asin( s.vn/s.v );
  533.       tan_kulma=wc_anna_theta(s);
  534.       suuntero=tan_kulma- v_kulma;
  535.  
  536.     }
  537.   }
  538.   alpha=suuntero;
  539. }
  540.  
  541. void wc_sladiraj(double nykyv){
  542.   if (r3==0){        // Rajoitetaan sladia suoralla
  543.     if (vc/nykyv>(1+accslad)){
  544.       vc=nykyv*(1+accslad);
  545.       if (vc<nykyv+12) vc=nykyv+12;
  546.     }
  547.     else if (vc/nykyv<(1-decslad))vc=nykyv*(1-decslad);
  548.   }
  549.   else{              //raj. sladia mutkassa
  550.     if (vc/nykyv>(1+curvslad)&&nykyv>30){
  551.       if (vc/nykyv<1.3)
  552.       vc=nykyv*(1+curvslad);
  553.     }
  554.     else if (vc/nykyv<(1-curvslad)&&r3!=0) {
  555.       vc=nykyv*(1-curvslad);
  556.     }
  557.   }
  558. }
  559.  
  560. void wc_passing(situation s){
  561. int i,vaisto_suunta=-9999;
  562. double v,d;
  563. double d_alfa,v_alfa;
  564. double x,y,vx,vy;
  565.  
  566.   if (ohitus==0){
  567.     for(i=0;i<3;i++) if (s.nearby[i].who<16){
  568.       x=s.nearby[i].rel_x;
  569.       y=s.nearby[i].rel_y;
  570.       vx=s.nearby[i].rel_xdot;
  571.       vy=s.nearby[i].rel_ydot;
  572.       d=sqrt(wc_sqr(x)+wc_sqr(y));
  573.       v=sqrt(wc_sqr(vx)+wc_sqr(vy));
  574.  
  575.       if (y!=0) d_alfa=atan(x/y);else d_alfa=x/(.01+wc_abs(x))*pii/2;
  576.       if (vy!=0)v_alfa=atan(vx/vy);else v_alfa=x/(.01+wc_abs(x))*pii/2;
  577.  
  578.       if(  (v/(d+.01)>.7 || d<CARWID )
  579.        && vy<0
  580.        && d_alfa-atan(2*CARWID / (d+.001)) <v_alfa
  581.        && d_alfa+atan(2*CARWID / (d+.001)) >v_alfa
  582.        && v/s.v<.9) {
  583.  
  584.     if (s.cur_rad==0&&s.to_end>nex_yli){ // jos suoralla, v„istet„„n
  585.       if (vaisto_suunta<-900) vaisto_suunta=0;
  586.       if (s.nex_rad<0) // k„„nnyt„n mieluummin vasemmalle
  587.         if (s.to_lft<15||
  588.            (s.to_end-nex_yli>300 && d_alfa<0) ) {
  589.           vaisto_suunta--;// jos auto vasemmalla, v„ist” oikealle
  590.         }
  591.         else { vaisto_suunta++; /*break;*/ }
  592.       else
  593.         if (s.to_rgt<15 ||
  594.            (s.to_end-nex_yli>300 && d_alfa>0) ){
  595.           vaisto_suunta++;
  596.         }
  597.         else { vaisto_suunta--;/* break;*/ }
  598.       }
  599.     else // jos ollaan mutkassa, jarrutetaan
  600.       vc=wc_minimum(s.v-cos(v_alfa)*sqrt(wc_sqr(vx)+wc_sqr(vy)),vc);
  601.       }
  602.       if (i==2&&vaisto_suunta>-800) {
  603.     if (vaisto_suunta<0) { ohitus=-1;alpha-=.2;}
  604.     else if (vaisto_suunta>0) {ohitus=1;alpha+=.2;}
  605.     else if ( (s.nex_rad<0&&s.to_lft>15)
  606.            || (s.nex_rad>0&&s.to_rgt<15) ) { alpha+=.2; ohitus=1; }
  607.     else { alpha-=.2;ohitus=-1; }
  608.       }
  609.     }
  610.   }
  611.   else if (ohitus!=0){
  612.     if ( s.cur_rad!=0||s.to_end<nex_yli
  613.      || ohitus<0&&s.to_rgt<5
  614.      || ohitus>0&&s.to_lft<5 )
  615.       ohitus=0;
  616.  
  617.     if (ohitus<0){
  618.       ohitus--;
  619.       if (alpha>-.2) alpha-=.2;
  620.     }
  621.     else if (ohitus>0){
  622.       ohitus++;
  623.       if (alpha<.2) alpha+=.2;
  624.     }
  625.     if (wc_abs(ohitus)>=15) ohitus=0;
  626.   }
  627. }
  628.  
  629.  
  630.  
  631. con_vec WappuCar(situation& s)// * * * * * *  PŽŽOHJELMA * * *  * *  * * * *  * *
  632. {
  633.   if (init_flag){                 // kerrotaan robotin nimi,
  634.     my_name_is(name);       // vain 1. kierroksella
  635.     r3=nex_r3;
  636.     result.vc=80;result.alpha=0;
  637.     init_flag=false;
  638.     return result;
  639.   }
  640.  
  641.   if (s.starting) wc_track_init(); // lasketaan radan ominaisuuksia
  642.  
  643.   // varmistutaan ett„ tiedet„„n miss„ ollaan menossa
  644.   while (s.cur_rad!=curves[n].r||s.nex_rad!=curves[(n+1>NSEG?0:n+1)].r){
  645.     n++;
  646.     if (n>=NSEG) n=0;
  647.   }
  648.  
  649. //  if (!no_display) piirramit(s.v);
  650.  
  651.   // lasketaan esim. r1,r2, nex_samat, kulmlopp, nykyv
  652.   wc_laskeperusjutut(s);
  653.  
  654.   // seuraavassa lasketaan ajolinjojen s„teet ja ylitys, jos mahdollista
  655.   wc_calc_driving_line(s); // l. r3, nex_r3, yli ja nex_yli
  656.  
  657.   // lasketaan uusi "velocity commanded"
  658.   wc_speed(s);
  659.  
  660.   // uusi alpha
  661.   wc_alpha(s);
  662.  
  663.   // v„istet„„n t”rm„yskurssilla olevia autoja
  664.   wc_passing(s);
  665.  
  666.   // rajoitetaan renkaitten suttaamista ja lukittumista
  667.   // prevent tire locking and sliding
  668.   wc_sladiraj(s.v);
  669.  
  670.   // estet„„n ulosmenoa ja tullaan takaisin tielle jos ollaan ulkona
  671.   // this is my own stuck() function
  672.   if (s.to_lft<0||s.to_rgt<0){
  673.     if (s.v<40) vc=s.v+20;else vc=s.v;
  674.     if (s.to_lft<0&&s.vn>-8) alpha=-.3;
  675.     if (s.to_rgt<0&&s.vn<8) alpha=.3;
  676.   }
  677.   if ((s.backward)||(ulkona)){
  678.     ulkona=true;
  679.     if (s.to_rgt<0||s.to_lft<0) alpha=0;
  680.     else{
  681.       if (s.vn<0) alpha=.5;
  682.       else alpha=-.5;
  683.       if (s.vn/s.v<.5&&s.vn/s.v>-.5) ulkona=false;
  684.     }
  685.   }
  686.  
  687.   // palautetaan lasketut arvot
  688.   result.vc = vc;//vc*(vc<102.7)+ (vc>102.7)*102.7;
  689.   result.alpha = alpha;
  690.   return result;
  691. }
  692.  
  693.